

#include <map>
#include "finder.h"

/***********************************************************************************/

class vertex_t final {
  const vertex* pv = nullptr;

public:
  inline vertex_t() noexcept
    : vertex_t(nullptr) {
  }
  inline vertex_t(vertex_t&& r) noexcept {
    pv = r.pv; r.pv = nullptr;
  }
  inline vertex_t(const vertex* v) noexcept
    : pv(v) {
  }
  inline vertex_t(const vertex_t& r) noexcept
    : vertex_t(r.pv) {
  }
  inline void update(const vertex* v) {
    auto p = pv; pv = v; delete p;
  }

public:
  inline const vertex* operator->() const {
    return get();
  }
  inline const vertex* get() const {
    return pv;
  }
  inline void  operator=(const vertex_t& r) noexcept {
    pv = r.pv;
  }
  inline bool  operator<(const vertex_t& v) const {
    if (pv->f < v.pv->f) return true;
    return (pv->f == v.pv->f) ? pv->i < v.pv->i : false;
  }
};

/***********************************************************************************/

#define not_found(i, q) (i == q.end())
static thread_local size_t depth_max = PF_DEPTH;
static thread_local std::map<size_t, vertex_t> close_queue;
static thread_local std::map<vertex_t, size_t> priority_queue;

/***********************************************************************************/

static const vertex* pop_vertex() {
  const vertex* pv = nullptr;
  if (!priority_queue.empty()) {
    auto iter = priority_queue.begin();
    pv = iter->first.get();
    priority_queue.erase(iter);
  }
  return pv;
}

/***********************************************************************************/

static void find_reset() {
  for (auto& iter : close_queue) {
    delete iter.second.get();
  }
  close_queue.clear();
  priority_queue.clear();
}

/***********************************************************************************/

static void push_vertex(const vertex* pv) {
  const vertex_t v(pv);
  auto iter = close_queue.find(pv->i);
  if (not_found(iter, close_queue)) {
    close_queue[v->i] = v;
    priority_queue[v] = v->i;
    return;
  }
  if (iter->second->f <= v->f) {
    delete pv;
    return;
  }
  priority_queue.erase(iter->second);
  iter->second.update(pv);
  priority_queue[v] = v->i;
}

/***********************************************************************************/

void push_vertex(size_t index, const vertex* pv, size_t g, size_t h) {
  if (pv->i != index) {
    vertex* p = vertex::create(index, pv);
    p->g = g;
    p->f = g + h;
    push_vertex(p);
  }
}

/***********************************************************************************/

void set_options(options what, size_t value) {
  switch (what) {
  case options::depth: depth_max = value;
    break;
  }
}

/***********************************************************************************/

const vertex* path_find(size_t begin, size_t end, neighbors fn, void* ud) {
  find_reset();
  size_t depth = depth_max;
  const vertex* pv = vertex::create(end, nullptr);
  if (pv) {
    push_vertex(pv);
    while (depth) {
      pv = pop_vertex();
      if (!pv || pv->i == begin) break;
      if (--depth == 0) { pv = nullptr; }
      else { fn(pv, begin, ud); }
    }
  }
  return pv;
}

/***********************************************************************************/
